/******************************************************************************
秋夜静，独坐对残灯。
啼笑非非谁识我，坐行梦梦尽缘君，何所慰消沉。
风卷雨，雨复卷侬心。
心似欲随风雨去，茫茫大海任浮沉。
无爱亦无恨。
******************************************************************************/
#include "Effects/kbRGB/kbRGB_Angle.h"
#include <stdio.h>
#include <math.h>

static const uint16_t RotateMatrixAngle[KBRGB_ANGLE_SIZE][KBRGB_ANGLE_SIZE] = {
{0x0000, 0x0800, 0x1000, 0x1800, 0x2000, 0x2800, 0x3000, 0x3800, 0x4000, 0x4800, 0x5000, 0x5800, 0x6000, 0x6800, 0x7000, 0x7800, 0x8000, 0x8800, 0x9000, 0x9800, 0xA000},
{0x0967, 0x0AB3, 0x106A, 0x1849, 0x2038, 0x282D, 0x3025, 0x3820, 0x401C, 0x4819, 0x5016, 0x5814, 0x6013, 0x6811, 0x7010, 0x780F, 0x800E, 0x880D, 0x900C, 0x980C, 0xA00B},
{0x1167, 0x10FD, 0x16B3, 0x1C86, 0x226A, 0x2A57, 0x3249, 0x3A3F, 0x4038, 0x4832, 0x502D, 0x5829, 0x6025, 0x6822, 0x7020, 0x781E, 0x801C, 0x881A, 0x9019, 0x9818, 0xA016},
{0x1967, 0x191E, 0x1CE1, 0x20B3, 0x2893, 0x2E7B, 0x346A, 0x3C5C, 0x4452, 0x4A49, 0x5242, 0x5A3D, 0x6238, 0x6A33, 0x7230, 0x7A2D, 0x822A, 0x8A28, 0x9025, 0x9823, 0xA022},
{0x2167, 0x212F, 0x22FD, 0x28D4, 0x2CB3, 0x329A, 0x3886, 0x4076, 0x466A, 0x4E5F, 0x5657, 0x5C4F, 0x6449, 0x6C44, 0x743F, 0x7C3B, 0x8238, 0x8A34, 0x9232, 0x9A2F, 0xA22D},
{0x2967, 0x293A, 0x2B10, 0x2EEC, 0x32CD, 0x38B3, 0x3E9F, 0x448E, 0x4A80, 0x5274, 0x586A, 0x6061, 0x685A, 0x6E54, 0x764E, 0x7E49, 0x8645, 0x8C41, 0x943E, 0x9C3A, 0xA438},
{0x3167, 0x3142, 0x331E, 0x34FD, 0x38E1, 0x3EC8, 0x42B3, 0x48A2, 0x5093, 0x5686, 0x5C7B, 0x6472, 0x6A6A, 0x7263, 0x785C, 0x8057, 0x8852, 0x904D, 0x9649, 0x9E46, 0xA642},
{0x3967, 0x3947, 0x3B28, 0x3D0B, 0x40F1, 0x44D9, 0x48C5, 0x4EB3, 0x54A4, 0x5A97, 0x608B, 0x6881, 0x6E79, 0x7671, 0x7C6A, 0x8464, 0x8A5E, 0x9259, 0x9A55, 0xA050, 0xA84D},
{0x4167, 0x414B, 0x412F, 0x4515, 0x46FD, 0x4AE7, 0x50D4, 0x54C3, 0x5AB3, 0x60A6, 0x669A, 0x6C90, 0x7286, 0x7A7E, 0x8076, 0x8870, 0x8E6A, 0x9664, 0x9C5F, 0xA45B, 0xAC57},
{0x4967, 0x494E, 0x4935, 0x4B1E, 0x4F08, 0x52F3, 0x56E1, 0x5AD0, 0x60C1, 0x64B3, 0x6AA7, 0x709D, 0x7893, 0x7E8A, 0x8482, 0x8A7B, 0x9275, 0x986F, 0xA06A, 0xA865, 0xAE60},
{0x5167, 0x5151, 0x513A, 0x5325, 0x5710, 0x58FD, 0x5CEC, 0x60DC, 0x66CD, 0x6AC0, 0x70B3, 0x76A9, 0x7C9F, 0x8296, 0x888E, 0x9086, 0x9680, 0x9C79, 0xA474, 0xAA6F, 0xB26A},
{0x5967, 0x5953, 0x593E, 0x5B2A, 0x5D18, 0x6106, 0x64F5, 0x68E6, 0x6CD7, 0x70CA, 0x76BE, 0x7CB3, 0x82AA, 0x88A0, 0x8E98, 0x9491, 0x9A8A, 0xA083, 0xA87D, 0xAE78, 0xB673},
{0x6167, 0x6154, 0x6142, 0x632F, 0x651E, 0x690D, 0x6AFD, 0x6EEE, 0x72E1, 0x78D4, 0x7CC8, 0x82BD, 0x86B3, 0x8CAA, 0x92A2, 0x989A, 0xA093, 0xA68C, 0xAC86, 0xB281, 0xBA7B},
{0x6967, 0x6956, 0x6945, 0x6B34, 0x6D23, 0x6F13, 0x7304, 0x76F6, 0x7AE9, 0x7EDD, 0x82D1, 0x88C7, 0x8CBD, 0x92B3, 0x98AB, 0x9EA3, 0xA49C, 0xAA95, 0xB08F, 0xB889, 0xBE84},
{0x7167, 0x7157, 0x7147, 0x7337, 0x7528, 0x7719, 0x790B, 0x7CFD, 0x80F1, 0x84E5, 0x88D9, 0x8ECF, 0x92C5, 0x98BC, 0x9EB3, 0xA4AC, 0xAAA4, 0xB09D, 0xB697, 0xBC91, 0xC28B},
{0x7967, 0x7958, 0x7949, 0x7B3A, 0x7D2C, 0x7F1E, 0x8110, 0x8503, 0x88F7, 0x8AEC, 0x90E1, 0x94D6, 0x98CD, 0x9EC4, 0xA4BB, 0xA8B3, 0xAEAC, 0xB4A5, 0xBA9F, 0xC099, 0xC893},
{0x8167, 0x8159, 0x814B, 0x833D, 0x832F, 0x8722, 0x8915, 0x8B09, 0x8EFD, 0x92F2, 0x96E7, 0x9ADD, 0xA0D4, 0xA4CB, 0xAAC3, 0xAEBB, 0xB4B3, 0xBAAD, 0xC0A6, 0xC6A0, 0xCC9A},
{0x8967, 0x895A, 0x894D, 0x8B3F, 0x8B33, 0x8D26, 0x911A, 0x930E, 0x9703, 0x98F8, 0x9CEE, 0xA0E4, 0xA6DB, 0xAAD2, 0xB0CA, 0xB4C2, 0xBABA, 0xC0B3, 0xC6AD, 0xCAA7, 0xD0A1},
{0x9167, 0x915B, 0x914E, 0x9142, 0x9335, 0x9529, 0x971E, 0x9B12, 0x9D08, 0xA0FD, 0xA4F3, 0xA8EA, 0xACE1, 0xB0D8, 0xB6D0, 0xBAC8, 0xC0C1, 0xC6BA, 0xCAB3, 0xD0AD, 0xD6A7},
{0x9967, 0x995B, 0x994F, 0x9944, 0x9B38, 0x9D2D, 0x9F21, 0xA117, 0xA50C, 0xA902, 0xAAF8, 0xAEEF, 0xB2E6, 0xB8DE, 0xBCD6, 0xC0CE, 0xC6C7, 0xCAC0, 0xD0BA, 0xD6B3, 0xDCAE},
{0xA167, 0xA15C, 0xA151, 0xA145, 0xA33A, 0xA52F, 0xA725, 0xA91A, 0xAD10, 0xAF07, 0xB2FD, 0xB6F4, 0xBAEC, 0xBEE3, 0xC2DC, 0xC8D4, 0xCCCD, 0xD0C6, 0xD6C0, 0xDCB9, 0xE2B3},
};

// 此处通过 uint32_t 返回数据由于数据丢失会产生奇特的效果
static inline uint16_t _kbRGB_Angle(float x, float y, float cx, float cy)
{
    double angle    = (atan2(x - cx, y - cy)) * 180 / 3.14159265359;
    double distance = sqrt(pow(cx - x, 2) + pow(cy - y, 2));

    struct kbRGB_Pixel_Int piont;
#if 0
    piont.angle = (uint16_t)(angle*8.0f);     // 90*8 = 720:10bit[1024]
    piont.radius = (uint16_t)(distance);
    piont.radius = (uint16_t)(distance*2.0); // sqrt(21*21)*2 = 29.7*2 = 60: 6bit [64]
#else
    const float kbRGB_angls_pre = (float)(0x1<<KBRGB_ANGLE_INT_PRE);
    const float kbRGB_radius_pre = (float)(0x1<<KBRGB_RADIUS_INT_PRE);
    piont.angle = (uint16_t)(angle*kbRGB_angls_pre);     // 90*8 = 720:10bit[1024]
    piont.radius = (uint16_t)(distance*kbRGB_radius_pre); // sqrt(21*21)*2 = 29.7*2 = 60: 6bit [64]
#endif
    return piont.pixel;
}
//static inline uint16_t _kbRGB_Angle_Only(float x, float y, float cx, float cy)
//{
//    double angle    = (atan2(x - cx, y - cy)) * 180 / 3.14159265359;
//    const float kbRGB_angls_pre = (float)(0x1<<KBRGB_XY2ANGLE_PRE);
//    uint16_t Angle = (uint16_t)(angle*kbRGB_angls_pre);     // 90*8 = 720:10bit[1024]
//    return Angle;
//}
/*************************************************************************************************
* 为节省空间占用,仅计算 0-90°范围,其它角度根据象限间的关系重新计算
* 为节保留 精度并加快运算速度,将处理后的结果保存为整型，精度为 1/64
**************************************************************************************************/
static void kbRGB_Angle(const uint16_t cols, const uint16_t rows)
{
    float cx = 0;//(cols - 1) * 0.5;
    float cy = 0;//(rows - 1) * 0.5;
    struct kbRGB_Pixel_Int matrix[128][128];
    for (uint32_t col_id = 0; col_id < cols; col_id++)
    {
        for (uint32_t row_id = 0; row_id < rows; row_id++)
        {
            matrix[col_id][row_id].pixel = _kbRGB_Angle(col_id, row_id, cx, cy);
        }
    }
    //printf("const uint16_t RotateMatrix[%d][%d] = {\r\n", MATRIX_SIZE, MATRIX_SIZE);
    //printf("const uint16_t RotateMatrixAngle[MATRIX_SIZE][MATRIX_SIZE] = {\r\n");
    //printf("const uint32_t RotateMatrixAngle[%d][%d] = {\r\n", cols, rows);
    printf("static const uint16_t RotateMatrixAngle[KBRGB_ANGLE_SIZE][KBRGB_ANGLE_SIZE] = {\r\n");
    for(int y=0; y<rows; y++)
    {
        printf("{");
        for(int x=0; x<cols; x++)
        {
            //printf("%4d, ",  (int)matrix[y][x].angle);
            //printf("0x%04X, ",  matrix[y][x].pixel);
            printf("0x%04X",  matrix[y][x].pixel);
            if(x<(cols-1)) printf(", ");
        }
        printf("},\r\n");
    }
    printf("};\r\n");
#if 1
    printf("static const uint16_t angle[MATRIX_ANGLE_SIZE][MATRIX_ANGLE_SIZE] = {\r\n");
    for(int y=0; y<rows; y++)
    {
        printf("{");
        for(int x=0; x<cols; x++)
        {
            //printf("%4d, ",  (int)matrix[y][x].angle);
            printf("%3d, ",  matrix[y][x].angle);
        }
        printf("},\r\n");
    }
    printf("};\r\n");
    printf("static const uint16_t radius[MATRIX_ANGLE_SIZE][MATRIX_ANGLE_SIZE] = {\r\n");
    for(int y=0; y<rows; y++)
    {
        printf("{");
        for(int x=0; x<cols; x++)
        {
            //printf("%4d, ",  (int)matrix[y][x].angle);
            printf("%3d, ",  matrix[y][x].radius);
        }
        printf("},\r\n");
    }
    printf("};\r\n");
#endif
    fflush(stdout);
}
//static void kbRGB_Angle_Only(const uint16_t cols, const uint16_t rows)
//{
//    float cx = 0;//(cols - 1) * 0.5;
//    float cy = 0;//(rows - 1) * 0.5;
//    uint8_t matrix[128][128];
//    uint16_t col_id, row_id;
//    for (row_id = 0; row_id < rows; row_id++)
//    {
//        for (col_id = 0; col_id < cols; col_id++)
//        {
//            matrix[row_id][col_id] = _kbRGB_Angle_Only(col_id, row_id, cx, cy);
//        }
//    }
//    printf("static const uint8_t XY2Angle[KBRGB_ANGLE_SIZE*2][KBRGB_ANGLE_SIZE*2] = {\r\n");
//    for (row_id = 0; row_id < rows; row_id++)
//    {
//        printf("{");
//        for (col_id = 0; col_id < cols; col_id++)
//        {
//            //printf("%4d, ",  (int)matrix[y][x].angle);
//            //printf("0x%04X, ",  matrix[y][x].pixel);
//            printf("0x%02X",  matrix[row_id][col_id]);
//            if(col_id<(cols-1)) printf(", ");
//        }
//        printf("},\r\n");
//    }
//    printf("};\r\n");
//#if 1
//    printf("static const uint16_t angle[MATRIX_ANGLE_SIZE][MATRIX_ANGLE_SIZE] = {\r\n");
//    for (row_id = 0; row_id < rows; row_id++)
//    {
//        printf("{");
//        for (col_id = 0; col_id < cols; col_id++)
//        {
//            //printf("%4d, ",  (int)matrix[y][x].angle);
//            printf("%3d, ",  matrix[row_id][col_id]);
//        }
//        printf("},\r\n");
//    }
//    printf("};\r\n");
//#endif
//    fflush(stdout);
//}
void kbRGB_Angle2_Int(void)
{
    kbRGB_Angle(KBRGB_ANGLE_SIZE, KBRGB_ANGLE_SIZE);
    //kbRGB_Angle_Only(KBRGB_ANGLE_SIZE*2, KBRGB_ANGLE_SIZE*2);
}
uint32_t kbRGB_Angle_IntGet(const int16_t x, const int16_t y)
{
    struct kbRGB_Pixel_Int Piont = {0};
    struct kbRGB_Pixel piont = {.angle=180<<KBRGB_ANGLE_INT_PRE, .radius=0xFFFF};
    if((x>=KBRGB_ANGLE_SIZE) || (x<=-KBRGB_ANGLE_SIZE)) return piont.pixel;
    if((y>=KBRGB_ANGLE_SIZE) || (y<=-KBRGB_ANGLE_SIZE)) return piont.pixel;
    if((x>=0) && (y>=0)) // 第一象限
    {
        Piont.pixel = RotateMatrixAngle[y][x];
        piont.angle = Piont.angle;
        piont.radius = Piont.radius;
    }
    if((x<0) && (y>=0)) // 第二象限
    {
        Piont.pixel = RotateMatrixAngle[y][-x];
        piont.angle = Piont.angle;
        piont.radius = Piont.radius;
        //piont.angle = (uint32_t)(180*8) - piont.angle;
        piont.angle = ((uint32_t)180<<KBRGB_ANGLE_INT_PRE) - piont.angle;
    }
    if((x<0) && (y<0)) // 第三象限
    {
        Piont.pixel = RotateMatrixAngle[-y][-x];
        piont.angle = Piont.angle;
        piont.radius = Piont.radius;
        //piont.angle = (uint32_t)(180*8) + piont.angle;
        piont.angle = ((uint32_t)180<<KBRGB_ANGLE_INT_PRE) + piont.angle;
    }
    if((x>=0) && (y<0)) // 第四象限
    {
        Piont.pixel = RotateMatrixAngle[-y][x];
        piont.angle = Piont.angle;
        piont.radius = Piont.radius;
        //piont.angle = (uint32_t)(360*8) - piont.angle;
        piont.angle = ((uint32_t)360<<KBRGB_ANGLE_INT_PRE) - piont.angle;
    }
    return piont.pixel;
}
//float CarmackSqrt (float x)
//{
//       float xhalf = 0.5f * x;

//       int i = *(int*)&x;           // get bits for floating VALUE
//       i = 0x5f3759df - (i>>1);     // gives initial guess y0
//       x = *(float*)&i;             // convert bits BACK to float
//       x = x*(1.5f - xhalf*x*x);    // Newton step, repeating increases accuracy
//       x = x*(1.5f - xhalf*x*x);    // Newton step, repeating increases accuracy
//       x = x*(1.5f - xhalf*x*x);    // Newton step, repeating increases accuracy
//       return (1 / x);
//}
//unsigned short kb_sqrt(unsigned long a)
//{
//   unsigned long rem = 0;
//   unsigned long root = 0;
//   unsigned long divisor = 0;
//   for(int i=0; i<16; i++)

//   {
//     root <<= 1;
//     rem = ((rem << 2) + (a >> 30));
//     a <<= 2;
//     divisor = (root<<1) + 1;
//     if(divisor <= rem){
//       rem -= divisor;
//       root++;
//     }
//   }
//   return (unsigned short)(root);
//}
uint32_t kbRGB_RadiusGet(const int8_t x, const int8_t y)
{
    int32_t xx = x*256;
    int32_t yy = y*256;
    uint32_t radius = kb_sqrt(xx*xx + yy*yy);
    return radius;
}
uint16_t _kbRGB_Radius_IntGet(const int16_t x, const int16_t y)
{
    struct kbRGB_Pixel_Int Piont = {0};
    struct kbRGB_Pixel piont = {.angle=0, .radius=0xFFFF};
    if((x>=KBRGB_ANGLE_SIZE) || (x<=-KBRGB_ANGLE_SIZE)) return piont.pixel;
    if((y>=KBRGB_ANGLE_SIZE) || (y<=-KBRGB_ANGLE_SIZE)) return piont.pixel;
    if((x>=0) && (y>=0)) // 第一象限
    {
        Piont.pixel = RotateMatrixAngle[y][x];
        piont.angle = Piont.angle;
        piont.radius = Piont.radius;
    }
    if((x<0) && (y>=0)) // 第二象限
    {
        Piont.pixel = RotateMatrixAngle[y][-x];
        piont.angle = Piont.angle;
        piont.radius = Piont.radius;
        //piont.angle = (uint32_t)(180*8) - piont.angle;
        piont.angle = ((uint32_t)180<<KBRGB_ANGLE_INT_PRE) - piont.angle;
    }
    if((x<0) && (y<0)) // 第三象限
    {
        Piont.pixel = RotateMatrixAngle[-y][-x];
        piont.angle = Piont.angle;
        piont.radius = Piont.radius;
        //piont.angle = (uint32_t)(180*8) + piont.angle;
        piont.angle = ((uint32_t)180<<KBRGB_ANGLE_INT_PRE) + piont.angle;
    }
    if((x>=0) && (y<0)) // 第四象限
    {
        Piont.pixel = RotateMatrixAngle[-y][x];
        piont.angle = Piont.angle;
        piont.radius = Piont.radius;
        //piont.angle = (uint32_t)(360*8) - piont.angle;
        piont.angle = ((uint32_t)360<<KBRGB_ANGLE_INT_PRE) - piont.angle;
    }
    return piont.pixel;
}
static uint16_t kbRGB_Radius(const uint16_t x2, const uint16_t y2, const uint16_t radius)
{
    // 根据相似三角形等比公式知: r1/r2=x1/x2=y1/y2 ==> r2=r1*x2/x1 = r18y2/y1
    uint16_t rx2, ry2;
    uint16_t x1 = (x2&0xFF00)+1;
    uint16_t y1 = (y2&0xFF00)+1;
    rx2 = radius*x2/x1;
    ry2 = radius*y2/y1;
    return ((rx2+ry2)>>1);
}
uint16_t kbRGB_Radius_IntGet(const int16_t x, const int16_t y)
{
    uint16_t radius = 0xFFFF;
    struct kbRGB_Pixel_Int Piont = {0};
    const int16_t x1 = x>>8;
    const int16_t y1 = y>>8;
    if((x1>=KBRGB_ANGLE_SIZE) || (x1<=-KBRGB_ANGLE_SIZE)) return radius;
    if((y1>=KBRGB_ANGLE_SIZE) || (y1<=-KBRGB_ANGLE_SIZE)) return radius;
    if((x1>=0) && (y1>=0)) // 第一象限
    {
        Piont.pixel = RotateMatrixAngle[y1][x1];
        radius = kbRGB_Radius(x, y, Piont.radius);
    }
    if((x1<0) && (y1>=0)) // 第二象限
    {
        Piont.pixel = RotateMatrixAngle[y1][-x1];
        radius = kbRGB_Radius(-x, y, Piont.radius);
    }
    if((x1<0) && (y1<0)) // 第三象限
    {
        Piont.pixel = RotateMatrixAngle[-y1][-x1];
        radius = kbRGB_Radius(-x, -y, Piont.radius);
    }
    if((x1>=0) && (y1<0)) // 第四象限
    {
        Piont.pixel = RotateMatrixAngle[-y1][x1];
        radius = kbRGB_Radius(x, -y, Piont.radius);
    }
    return radius;
}
